#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"    //引入ROS2中nav_msgs功能包下的Odometry消息类型的头文件
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/utils.h"  //提供tf2::getEulerYPR函数
#include <cmath>

class GazeboHeartTrajectory : public rclcpp::Node {
private:
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_publisher_;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscription_;
    
    // 心形轨迹参数
    std::vector<std::pair<double, double>> heart_points_;
    size_t current_target_index_{0};
    double k_linear_{1.0};        // 线性速度增益
    double k_angular_{1.5};       // 角速度增益
    double point_reached_threshold_{0.15};  // 目标点到达阈值
    double max_speed_{1.3};       // 最大速度
    double min_speed_{0.05};      // 最小速度
    rclcpp::Time last_log_time_;
    bool first_odom_received_{false};

    

public:
    GazeboHeartTrajectory() : Node("gazebo_heart_trajectory") {
        // 生成心形轨迹点
        generate_heart_points();
        RCLCPP_INFO(this->get_logger(), "生成了%zu个点", heart_points_.size());
        
        // 初始化发布者和订阅者
        velocity_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
            "/cmd_vel", 10); 
        odom_subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "/odom", 10, 
            std::bind(&GazeboHeartTrajectory::on_odom_received_, this, std::placeholders::_1));
            
        RCLCPP_INFO(this->get_logger(), "咩咩开始沿着心型轨迹移动");
    }

private:
    // 生成心形轨迹点
    void generate_heart_points() {
        const int num_points = 200;  
        const double scale = 1.0;    
        const double center_x = 0.0; // Gazebo通常以原点为中心
        const double center_y = 0.0;

        for (int i = 0; i < num_points; i++) {
            double t = static_cast<double>(i) / num_points * 2 * M_PI;
            // 心形线参数方程
            double x = 16 * pow(sin(t), 3);
            double y = 13 * cos(t) - 5 * cos(2*t) - 2 * cos(3*t) - cos(4*t);
            
            // 缩放和平移到Gazebo场景中心
            x = x * scale / 16.0 + center_x;
            y = y * scale / 16.0 + center_y;
            heart_points_.emplace_back(x, y);
        }
    }

    // 从四元数获取偏航角（yaw）
    double get_yaw_from_quaternion(const geometry_msgs::msg::Quaternion& quat) {
        tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w);//将 ROS 消息类型的四元数（geometry_msgs::msg::Quaternion）
                                                        //转换为 TF2 库（ROS 中用于坐标变换的库）的 
                                                        //tf2::Quaternion 类型，
                                                        //以便使用 TF2 提供的姿态转换工具
        double roll, pitch, yaw;
        tf2::getEulerYPR(q, yaw, pitch, roll);
        return yaw;
    }

    void on_odom_received_(const nav_msgs::msg::Odometry::SharedPtr odom) {
        if (!first_odom_received_) {
            first_odom_received_ = true;
            last_log_time_ = this->now();
            RCLCPP_INFO(this->get_logger(), "第一次获取odom");
        }
        
        auto msg = geometry_msgs::msg::Twist();
        
        // 获取当前位置和姿态
        double current_x = odom->pose.pose.position.x;
        double current_y = odom->pose.pose.position.y;
        double current_theta = get_yaw_from_quaternion(odom->pose.pose.orientation);
        
        // 获取当前目标点
        double target_x = heart_points_[current_target_index_].first;
        double target_y = heart_points_[current_target_index_].second;
        
        // 计算到目标点的距离和角度误差
        double dx = target_x - current_x;
        double dy = target_y - current_y;
        double distance = sqrt(dx*dx + dy*dy);
        double target_angle = atan2(dy, dx);
        double angle_error = target_angle - current_theta;
        
        // 角度误差归一化到[-π, π]
        while(angle_error<-M_PI){
            angle_error+=2*M_PI;
        }
        while(angle_error>M_PI){
            angle_error-=2*M_PI;
        }
        
        // 检查是否到达当前目标点
        if (distance < point_reached_threshold_) {
            current_target_index_ = (current_target_index_ + 1) % heart_points_.size();
            RCLCPP_INFO(this->get_logger(), "目标点已到达 %zu/%zu", 
                        current_target_index_, heart_points_.size());
        }
        
        // 控制逻辑
        if (fabs(angle_error) > 0.2) {
            // 优先转向对准目标方向
            msg.angular.z = k_angular_ * angle_error;
            msg.linear.x = min_speed_; 
        } else {
            // 角度误差较小，主要控制前进
            msg.linear.x = std::min(k_linear_ * distance, max_speed_);
            msg.angular.z = k_angular_ * angle_error; 
        }
        
        velocity_publisher_->publish(msg);
        
        // 定时日志输出
        auto now = this->now();
        if ((now - last_log_time_).seconds() > 1.0) {
            RCLCPP_INFO(this->get_logger(), 
                        "当前位置(%.2f, %.2f, %.2f) 当前目标(%.2f, %.2f) | 距离: %.2f, 角度茶: %.2f",
                        current_x, current_y, current_theta,
                        target_x, target_y, distance, angle_error);
            last_log_time_ = now;
        }
    }
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<GazeboHeartTrajectory>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}